float height;
float speed;
float heading;
- short dsta; // differential station
- float dage; // differential ...
+ short dsta; // differential station id
+ float dage; // differential data age
float pdop, hdop, vdop;
- char sat_used, sat_view;
+ char sat_used, sat_view, sat_count;
short rcr;
unsigned short timestamp_ms;
double distance;
struct sat_info sat_data[32];
} data_item;
+struct mtk_loginfo {
+ unsigned int bitmask;
+ int logLen;
+ int period, distance, speed; /* in 10:ths of sec, m, km/h */
+} mtk_loginfo;
+
/* *************************************** */
-#define TIMEOUT 1000
+#define TIMEOUT 1500
#define MTK_BAUDRATE 115200
#define MTK_BAUDRATE_M241 38400
static char *csv_file; /* csv ? command option */
static int is_m241=0;
-static unsigned int bmask = 0x000e0e7f;
-static unsigned int mlog_period, mlog_distance, mlog_speed; /* in 10:ths of sec, m, km/h */
-
+struct mtk_loginfo mtk_info;
const char LIVE_CHAR[4] = {'-', '\\','|','/'};
const char TEMP_DATA_BIN[]= "data.bin";
const char CMD_LOG_ENABLE[] = "$PMTK182,4*21\r\n";
const char CMD_LOG_FORMAT[] = "$PMTK182,2,2*39\r\n";
const char CMD_LOG_ERASE[] = "$PMTK182,6,1*3E\r\n";
+const char CMD_LOG_STATUS[] = "$PMTK182,2,7*3C\r\n";
-
+static int mtk_log_len(unsigned int bitmask);
static void mtk_rd_init(const char *fname);
static void file_init(const char *fname);
static void file_deinit(void) ;
static void file_read(void);
+static int mtk_parse_info(const unsigned char *data, int dataLen);
// Arguments for logg fetch 'mtk' command..
return cmdLen;
}
-static int do_cmd(const char *cmd, const char *expect, time_t timeout_sec) {
+
+static int do_cmd(const char *cmd, const char *expect, char **rslt, time_t timeout_sec) {
char line[256];
int len, done, loops, cmd_erase;
int expect_len;
if ( expect_len > 0 && strncmp(&line[1], expect, expect_len) == 0 ){
if ( cmd_erase && global_opts.debug_level > 0 ) printf("\n");
dbg(6, "NMEA command success !\n");
+ if ( (len - 4) > expect_len ){ // alloc and copy data segment...
+ if ( line[len-3] == '*' )
+ line[len-3] = '\0';
+ // printf("Data segment: #%s#\n", &line[expect_len+1]);
+ if ( rslt ){
+ *rslt = xmalloc(len-3-expect_len+1);
+ strcpy(*rslt, &line[expect_len+1]);
+ }
+ }
done = 1;
} else if ( strncmp(line, "$PMTK", 5) == 0 ){
/* A quick parser for ACK packets */
fatal(MYNAME ": Failed to set baudrate !\n");
}
- rc = do_cmd("$PMTK605*31\r\n", "PMTK705", 10);
+ rc = do_cmd("$PMTK605*31\r\n", "PMTK705", NULL, 10);
if ( rc != 0 )
fatal(MYNAME ": This is not a MTK based GPS ! (or is it turned off ?)\n");
-}
+ }
static void mtk_rd_deinit(void){
dbg(3, "Closing port...\n");
xfree(port);
}
-int mtk_erase(void){
+static int mtk_erase(void){
+ int log_status, log_mask, err;
+ char *lstatus = NULL;
+
+ log_status = 0;
+ // check log status - is logging disabled ?
+ do_cmd(CMD_LOG_STATUS, "PMTK182,3,7,", &lstatus, 2);
+ if ( lstatus ){
+ log_status = atoi(lstatus);
+ dbg(3, "LOG Status '%s'\n", lstatus);
+ xfree(lstatus);
+ lstatus = NULL;
+ }
+
+ do_cmd(CMD_LOG_FORMAT, "PMTK182,3,2,", &lstatus, 2);
+ if ( lstatus ){
+ log_mask = strtoul(lstatus, NULL, 16);
+ dbg(3, "LOG Mask '%s' - 0x%.8x \n", lstatus, log_mask);
+ xfree(lstatus);
+ lstatus = NULL;
+ }
dbg(1, "Start flash erase..\n");
- do_cmd(CMD_LOG_DISABLE, "PMTK001,182,5,3", 1);
- gb_sleep(10*1000);
-
- do_cmd(CMD_LOG_FORMAT, "PMTK182,3,2", 2);
+ do_cmd(CMD_LOG_DISABLE, "PMTK001,182,5,3", NULL, 1);
gb_sleep(10*1000);
// Erase log....
- do_cmd(CMD_LOG_ERASE, "PMTK001,182,6", 30);
+ do_cmd(CMD_LOG_ERASE, "PMTK001,182,6", NULL, 30);
gb_sleep(100*1000);
- do_cmd(CMD_LOG_ENABLE, "PMTK001,182,4,3", 2);
-
+ if ( (log_status & 2) ){ // auto-log were enabled before..re-enable log.
+ err = do_cmd(CMD_LOG_ENABLE, "PMTK001,182,4,3", NULL, 2);
+ dbg(3, "re-enable log %s\n", err==0?"Success":"Fail");
+ }
return 0;
}
char cmd[256];
char *line = NULL;
unsigned char crc, *data = NULL;
- int cmdLen, j, bsize, i, len, ff_len, rc, init_scan;
+ int cmdLen, j, bsize, i, len, ff_len, rc, init_scan, retry_cnt, log_enabled;
unsigned int line_size, data_size, data_addr, addr, addr_max;
FILE *dout;
+ char *fusage = NULL;
+ log_enabled = 0;
+ init_scan = 0;
dout = fopen(TEMP_DATA_BIN, "wb");
if ( dout == NULL ){
fatal(MYNAME ": Can't create temporary file %s", TEMP_DATA_BIN);
}
dbg(1, "Download %s -> %s\n", port, TEMP_DATA_BIN);
- i = do_cmd(CMD_LOG_DISABLE, "PMTK001,182,5,3", 2);
- dbg(3, " ---- LOG DISABLE ---- %s\n", i==0?"Success":"Fail");
-
- addr_max = 0x200000-64*1024; // 16Mbit/2Mbyte/32x64kByte block.
- init_scan = 1;
+ // check log status - is logging disabled ?
+ do_cmd(CMD_LOG_STATUS, "PMTK182,3,7,", &fusage, 2);
+ if ( fusage ){
+ log_enabled = (atoi(fusage) & 2)?1:0;
+ dbg(3, "LOG Status '%s' -- log %s \n", fusage, log_enabled?"enabled":"disabled");
+ xfree(fusage);
+ fusage = NULL;
+ }
+
+ gb_sleep(10*1000);
+ if ( 1 || log_enabled ){
+ i = do_cmd(CMD_LOG_DISABLE, "PMTK001,182,5,3", NULL, 2);
+ dbg(3, " ---- LOG DISABLE ---- %s\n", i==0?"Success":"Fail");
+ }
+ gb_sleep(100*1000);
+
+ addr_max = 0;
+ // get flash usage, current log address..cmd only works if log disabled.
+ do_cmd("$PMTK182,2,8*33\r\n", "PMTK182,3,8,", &fusage, 2);
+ if ( fusage ){
+ addr_max = strtoul(fusage, NULL, 16);
+ if ( addr_max > 0 )
+ addr_max = addr_max - addr_max%65536 + 65535;
+ xfree(fusage);
+ }
+
+ if ( addr_max == 0 ) { // get flash usage failed...
+ addr_max = 0x200000-64*1024; // 16Mbit/2Mbyte/32x64kByte block. -- fixme Q1000-ng has 32Mbit
+ init_scan = 1;
+ }
bsize = 0x0400;
addr = 0x0000;
}
memset(line, '\0', line_size);
memset(data, '\0', data_size);
-
+ retry_cnt = 0;
while ( init_scan || addr < addr_max ) {
// generate - read address NMEA command, add crc.
crc ^= cmd[i];
cmdLen += snprintf(&cmd[cmdLen], sizeof(cmd)-cmdLen, "*%.2X\r\n", crc);
+mtk_retry:
do_send_cmd(cmd, cmdLen);
memset(line, '\0', line_size);
do {
rc = gbser_read_line(fd, line, line_size-1, TIMEOUT, 0x0A, 0x0D);
if ( rc != gbser_OK) {
- fatal(MYNAME "mtk_read(): Read error (%d)\n", rc);
+ if ( rc == gbser_TIMEOUT && retry_cnt < 3 ){
+ dbg(2, "\nRetry %d at 0x%.8x\n", retry_cnt, addr);
+ retry_cnt++;
+ goto mtk_retry;
+ } // else
+ fatal(MYNAME "mtk_read(): Read error (%d)\n", rc);
}
len = strlen(line);
dbg(8, "Read %d bytes: '%s'\n", len, line);
if ( len > 0 ) {
line[len] = '\0';
if ( strncmp(line, "$PMTK182,8", 10) == 0 ){// $PMTK182,8,00005000,FFFFFFF
+ retry_cnt = 0;
data_addr = strtoul(&line[11], NULL, 16);
fseek(dout, data_addr, SEEK_SET);
i = 20;
fclose(dout);
dbg(2, "\n");
- i = do_cmd(CMD_LOG_ENABLE, "PMTK001,182,4,3", 2);
- dbg(3, " ---- LOG ENABLE ----%s\n", i==0?"Success":"Fail");
-
+ if ( log_enabled ){
+ i = do_cmd(CMD_LOG_ENABLE, "PMTK001,182,4,3", NULL, 2);
+ dbg(3, " ---- LOG ENABLE ----%s\n", i==0?"Success":"Fail");
+ } else {
+ dbg(1, "Note !!! -- Logging is DISABLED !\n");
+ }
if ( line != NULL )
xfree(line);
if ( data != NULL )
if ( global_opts.masked_objective & TRKDATAMASK && trk_head == NULL ){
trk_head = route_head_alloc();
xasprintf(&trk_head->rte_desc, "Log every %.0f sec, %.0f m, %.0f km/h"
- , mlog_period/10., mlog_distance/10., mlog_speed/10.);
+ , mtk_info.period/10., mtk_info.distance/10., mtk_info.speed/10.);
track_add_head(trk_head);
}
gbfprintf(cd, "INDEX,%s%s", ((1<<RCR) & bitmask)?"RCR,":"",
((1<<UTC) & bitmask)?"DATE,TIME,":"" );
for (i=0;i<32;i++){
- if ( (1<<i) & bmask ){
+ if ( (1<<i) & bitmask ){
switch (i){
case RCR:
case UTC:
break;
}
}
- if ( i == SNR && (1<<SID) & bmask )
+ if ( i == SNR && (1<<SID) & bitmask )
gbfprintf(cd, "),");
}
gbfprintf(cd, "\n");
if ( bmask & (1<<SID) ){
int l, slen, do_sc = 0;
char sstr[40];
- for (l=0;l<32;l++){
- if ( itm->sat_data[l].id > 0 ){
- slen = 0;
- slen += sprintf(&sstr[slen], "%s%.2d"
- , itm->sat_data[l].used?"#":""
- , itm->sat_data[l].id);
- if ( bmask & (1<<ELEVATION) )
- slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].elevation);
- if ( bmask & (1<<AZIMUTH) )
- slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].azimut);
- if ( bmask & (1<<SNR) )
- slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].snr);
-
- gbfprintf(csvFile, "%s%s" , do_sc?";":"", sstr);
- do_sc = 1;
- }
+ for (l=0;l<itm->sat_count;l++){
+ slen = 0;
+ slen += sprintf(&sstr[slen], "%s%.2d"
+ , itm->sat_data[l].used?"#":""
+ , itm->sat_data[l].id);
+ if ( bmask & (1<<ELEVATION) )
+ slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].elevation);
+ if ( bmask & (1<<AZIMUTH) )
+ slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].azimut);
+ if ( bmask & (1<<SNR) )
+ slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].snr);
+
+ gbfprintf(csvFile, "%s%s" , do_sc?";":"", sstr);
+ do_sc = 1;
}
gbfprintf(csvFile, ",");
}
break;
case 1<<SID:
{
- int dLeft, l, sat_count, sid_size, sat_idx;
-
- // calculate data length after SID fields
- dLeft = 0;
- for (l = SNR; l <= DISTANCE; l++) {
- if (bmask && (1 << l))
- dLeft += log_type[l].size;
- }
- sat_count = 0;
- if ( dLeft > 0 )
- sat_count = le_read16(data + i + 2);
+ int sat_count, sat_idx, sid_size, l;
+
+ sat_count = le_read16(data + i + 2);
if ( sat_count > 32 )
sat_count = 32; // this can't happen ? or...
-
- sid_size = log_type[SID].size;
- if ( bmask & (1<<ELEVATION) )
- sid_size += log_type[ELEVATION].size;
- if ( bmask & (1<<AZIMUTH) )
- sid_size += log_type[AZIMUTH].size;
- if ( bmask & (1<<SNR) )
- sid_size += log_type[SNR].size;
+ itm.sat_count = sat_count;
+ sid_size = log_type[SID].size;
+ if ( sat_count > 0 ){ // handle 'Zero satellites in view issue'
+ if ( bmask & (1<<ELEVATION) )
+ sid_size += log_type[ELEVATION].size;
+ if ( bmask & (1<<AZIMUTH) )
+ sid_size += log_type[AZIMUTH].size;
+ if ( bmask & (1<<SNR) )
+ sid_size += log_type[SNR].size;
+ }
l = 0;
sat_idx = 0;
- while ( sat_count > 0 ){
+ do {
sat_id = data[i];
itm.sat_data[sat_idx].id = sat_id;
itm.sat_data[sat_idx].used = data[i + 1];
// get_word(&data[i+2], &smask); // assume - nr of satellites...
-
- if ( bmask & (1<<ELEVATION) )
- itm.sat_data[sat_idx].elevation = le_read16(data + i + 4);
- if ( bmask & (1<<AZIMUTH) )
- itm.sat_data[sat_idx].azimut = le_read16(data + i + 6);
- if ( bmask & (1<<SNR) )
- itm.sat_data[sat_idx].snr = le_read16(data + i + 8);
-
+
+ if ( sat_count > 0 ){
+ if ( bmask & (1<<ELEVATION) )
+ itm.sat_data[sat_idx].elevation = le_read16(data + i + 4);
+ if ( bmask & (1<<AZIMUTH) )
+ itm.sat_data[sat_idx].azimut = le_read16(data + i + 6);
+ if ( bmask & (1<<SNR) )
+ itm.sat_data[sat_idx].snr = le_read16(data + i + 8);
+ }
sat_idx++;
// duplicated checksum and length calculations...for simplicity...
for (l = 0; l < sid_size; l++)
crc ^= data[i + l];
i += sid_size;
sat_count--;
- } /* End: while ( sat_count > 0 ) */
+ } while ( sat_count > 0 );
}
continue; // dont do any more checksum calc..
break;
}
} /* for (bmap,...) */
- if ( ! is_m241 && ( data[i] != '*') ) {
- if ( global_opts.debug_level > 0 ) {
- int j;
- printf("Missing '*' !\n");
- for(j=0;j<dataLen;j++)
- printf("%.2x ", data[j]);
- printf("\n");
- }
- if ( data[0] == 0xaa && data[1] == 0xaa && data[2] == 0xaa && data[3] == 0xaa ){
- printf(" --- Log restart ?? skip 16 bytes ---\n");
- return 16;
- }
- }
- if ( ! is_m241 ) i++; // skip '*' separator
+ if ( ! is_m241 ){
+ if ( data[i] == '*' )
+ i++; // skip '*' separator
+ else
+ dbg(1,"Missing '*' !\n");
+ }
+ if ( memcmp(&data[0], &LOG_RST[0], 6) == 0
+ && memcmp(&data[12], &LOG_RST[12], 4) == 0 )
+ {
+ mtk_parse_info(data, dataLen);
+ dbg(1," Missed Log restart ?? skipping 16 bytes\n");
+ return 16;
+ }
if ( data[i] != crc ){
printf("%2d: Bad CRC %.2x != %.2x\n", count, data[i], crc);
/*
Description: Parse an info block
- Globals: bmask - may be affected if updated.
+ Globals: mtk_info - bitmask/period/speed/... may be affected if updated.
*/
static int mtk_parse_info(const unsigned char *data, int dataLen){
unsigned short cmd;
dbg(1, "# Log bitmask is: %.8x\n", bm);
if ( is_m241 )
bm &= 0x7fffffffU;
- if ( bmask != bm )
- dbg(1," ########## Bitmask Change %.8x -> %.8x ###########\n", bmask, bm);
- bmask = bm;
+ if ( mtk_info.bitmask != bm )
+ dbg(1," ########## Bitmask Change %.8x -> %.8x ###########\n", mtk_info.bitmask, bm);
+ mtk_info.bitmask = bm;
+ mtk_info.logLen = mtk_log_len(mtk_info.bitmask);
break;
case 0x03:
dbg(1, "# Log period change %.0f sec\n", cmd/10.);
- mlog_period = cmd;
+ mtk_info.period = cmd;
break;
case 0x04:
dbg(1, "# Log distance change %.1f m\n", cmd/10.);
- mlog_distance = cmd;
+ mtk_info.distance = cmd;
break;
case 0x05:
dbg(1, "# Log speed change %.1f km/h\n", cmd/10.);
- mlog_speed = cmd;
+ mtk_info.speed = cmd;
break;
case 0x06:
dbg(1, "# Log policy change 0x%.4x\n", cmd);
return 16;
}
+static int mtk_log_len(unsigned int bitmask){
+ int i, len;
+
+ /* calculate the length of a binary log item. */
+ len = 2; // add '*' + crc, holux would only be +1, oh, well...
+ for (i=0;i<32;i++){
+ if ( (1<<i) & bitmask ){
+ if ( (i == SID || i == ELEVATION || i == AZIMUTH || i == SNR) && (1<<SID) & bitmask )
+ len += log_type[i].size*32; // worst case, max sat. count..
+ else
+ len += log_type[i].size;
+ }
+ }
+ dbg(3, "Log item size %d bytes\n", len);
+ return len;
+}
+
/********************** File-in interface ********************************/
static void file_init_m241(const char *fname) {
static void file_read(void) {
long fsize, pos;
- int i, j, k, logLen, bLen;
+ int i, j, k, bLen;
unsigned char buf[512];
memset(buf, '\0', sizeof(buf));
dbg(1, "Default Bitmask %.8x, Log every %.0f sec, %.0f m, %.0f km/h\n",
mask, log_period/10., log_distance/10., log_speed/10.);
- bmask = mask;
- dbg(3, "Using initial bitmask %.8x for parsing the .bin file\n", bmask);
+ mtk_info.bitmask = mask;
+ dbg(3, "Using initial bitmask %.8x for parsing the .bin file\n", mtk_info.bitmask);
- mlog_period = log_period;
- mlog_distance = log_distance;
- mlog_speed = log_speed;
+ mtk_info.period = log_period;
+ mtk_info.distance = log_distance;
+ mtk_info.speed = log_speed;
}
pos = 0x200; // skip header...first data position
j = bLen;
pos += j;
- /* calculate the length of a binary log item. */
- logLen = 2; // add '*' + crc
- for (i=0;i<32;i++){
- if ( (1<<i) & bmask ){
- if ( (i == SID || i == ELEVATION || i == AZIMUTH || i == SNR) && (1<<SID) & bmask )
- logLen += log_type[i].size*32; // worst case, max sat. count..
- else
- logLen += log_type[i].size;
- }
- }
- dbg(3, "Log item size %d bytes\n", logLen);
+ mtk_info.logLen = mtk_log_len(mtk_info.bitmask);
+ dbg(3, "Log item size %d bytes\n", mtk_info.logLen);
if ( csv_file && *csv_file )
- mtk_csv_init(csv_file, bmask);
+ mtk_csv_init(csv_file, mtk_info.bitmask);
while ( pos < fsize && (bLen = fread(&buf[j], 1, sizeof(buf)-j, fl)) > 0 ){
bLen += j;
i = 0;
- while ( (bLen - i) >= logLen ){
+ while ( (bLen - i) >= mtk_info.logLen ){
k = 0;
if ( (bLen - i) >= 16 && memcmp(&buf[i], &LOG_RST[0], 6) == 0
&& memcmp(&buf[i+12], &LOG_RST[12], 4) == 0 )
{
/* End of 64k block segment -- realign to next data area */
- k = ((pos+logLen+1024)/0x10000) *0x10000 + 0x200;
+ k = ((pos+mtk_info.logLen+1024)/0x10000) *0x10000 + 0x200;
i = sizeof(buf);
if ( k <= pos ) {
k += 0x10000;
pos = k;
continue;
} else {
- k = mtk_parse(&buf[i], logLen, bmask);
+ k = mtk_parse(&buf[i], mtk_info.logLen, mtk_info.bitmask);
}
i += k;
projects / gpsbabel.git / commitdiff